{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "# Let's do all of our imports here.\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt, mpld3\n",
    "\n",
    "from pydrake.all import (RigidTransform, RollPitchYaw, RotationMatrix)\n",
    "\n",
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
    "\n",
    "if running_as_notebook:\n",
    "    mpld3.enable_notebook()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "QuFmomDOxoAR",
    "tags": []
   },
   "outputs": [],
   "source": [
    "num_points_per_side=7\n",
    "N = num_points_per_side*4\n",
    "x = np.arange(-1, 1, 2/num_points_per_side)\n",
    "half_width = 2\n",
    "half_height = 1\n",
    "top = np.vstack((half_width*x, half_height + 0*x))\n",
    "right = np.vstack((half_width + 0*x, -half_height*x))\n",
    "bottom = np.vstack((-half_width*x, -half_height + 0*x))\n",
    "left = np.vstack((-half_width + 0*x, half_height*x))\n",
    "p_m = np.vstack((np.hstack((top, right, bottom, left)), np.zeros((1, 4*num_points_per_side))))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def min_distance(x,y):\n",
    "    return np.sqrt(np.min((x - p_m[0,:])**2 + (y-p_m[1,:])**2))\n",
    "    \n",
    "ax = plt.subplot()\n",
    "ax.plot(p_m[0, :], p_m[1, :], 'b.')\n",
    "ax.fill(p_m[0, :], p_m[1, :], 'lightblue', alpha=0.5)\n",
    "\n",
    "X,Y = np.meshgrid(np.linspace(-3,3,150), np.linspace(-2,2,150))\n",
    "MinDistance = np.vectorize(min_distance)\n",
    "Z = MinDistance(X,Y)\n",
    "\n",
    "CS = ax.contour(X, Y, Z, 3)\n",
    "ax.clabel(CS, inline=True, fontsize=10);\n",
    "ax.axis('equal')\n",
    "ax.axis('off')\n",
    "plt.savefig(\"rectangle_points_distance.svg\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def signed_distance(x,y):\n",
    "    x = np.abs(x)\n",
    "    y = np.abs(y)\n",
    "    if (x>=2.0 and y>=1.0):\n",
    "        return np.sqrt((x-2)**2 + (y-1)**2)\n",
    "    if (y<=1.0 and x<=2.0):\n",
    "        return np.max([x-2, y-1])\n",
    "    if (x>2.0):\n",
    "        return x-2\n",
    "    return y-1\n",
    "\n",
    "ax = plt.subplot()\n",
    "p_m_closed = np.hstack((p_m, p_m[:,0].reshape(3,1)))  # close the box for this plot\n",
    "ax.plot(p_m_closed[0, :], p_m_closed[1, :], 'b-')\n",
    "ax.fill(p_m[0, :], p_m[1, :], 'lightblue', alpha=0.5)\n",
    "\n",
    "X,Y = np.meshgrid(np.linspace(-3,3,150), np.linspace(-2,2,150))\n",
    "SignedDistance = np.vectorize(signed_distance)\n",
    "Z = SignedDistance(X,Y)\n",
    "\n",
    "CS = ax.contour(X, Y, Z, 6)\n",
    "ax.clabel(CS, inline=True, fontsize=10);\n",
    "ax.axis('equal')\n",
    "ax.axis('off')\n",
    "plt.savefig(\"rectangle_signed_distance.svg\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = plt.subplot()\n",
    "#ax.plot(p_m[0, :], p_m[1, :], 'b.')\n",
    "ax.fill(p_m[0, :], p_m[1, :], 'lightsalmon', alpha=0.5)\n",
    "\n",
    "Nv = 15\n",
    "ax.plot(p_m[0, :Nv], p_m[1, :Nv], 'r.')\n",
    "c = np.array([3,2])\n",
    "o = np.hstack((p_m[:2,:Nv], c.reshape((2,1))))\n",
    "for i in range(Nv):\n",
    "    ax.plot([o[0,i], c[0]], [o[1, i],c[1]], 'r--', alpha=0.4)\n",
    "ax.fill(o[0, :], o[1, :], 'lightgreen', alpha=0.5)\n",
    "o_closed = np.hstack((c.reshape(2,1), o))\n",
    "ax.plot(o_closed[0,:], o_closed[1,:], 'g-')\n",
    "theta=0.8\n",
    "R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])\n",
    "c_box = c + np.matmul(R,0.3*np.array([[0,1,1,0], [-1,-1,1,1]])).T\n",
    "ax.fill(c_box[:,0], c_box[:,1], 'lightgray')\n",
    "ax.text(-.3,-.1,'object',color='r')\n",
    "ax.text(.5, 1.8, 'free space', color='g')\n",
    "ax.text(2.15,2.25,'camera')\n",
    "ax.axis('equal')\n",
    "ax.axis('off')\n",
    "plt.savefig(\"free_space.svg\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - Basic Pick and Place.ipynb",
   "provenance": [],
   "toc_visible": true
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}